---

---

---

---

---

• Angular velocity from gyroscope
• Distance from a “pedometer”
• WiFi signal strength measurements

They formulate their state space, their representation of the world, in terms of three parameters:

• Distance traveled per time step
• Angle that distance was traveled in
• WiFi signal strength

The output from the algorithm is the best estimate of this state space.

The distance traveled per time step and angle traveled can converted to the robots X-Y position through the following function:

The authors chose to use distance traveled and angle instead of more traditional X-Y coordinates because it makes the math easier at some points in the algorithm.¹

WiFi Measurement Function

Coming up with a way to predict WiFi measurements based in part on location is the most important step in the paper. This sort of measurement model is needed for all traditional SLAM solving techniques, and is difficult to formulate for RF signals like WiFi.² The problem with determining a measurement model for WiFi signal strength is that WiFi signal strength is highly dependent on building geometry and materials. If the robot knew enough about the environment to fully predict WiFi signal strength, it would already have a highly accurate and detailed map, and thus not be solving SLAM. To get around this problem, the authors of the paper do not try to perfectly predict the WiFi signal strength, they just try to make a good enough prediction and use it to compensate for the error in the IMU measurements.

To predict a single WiFi measurement, the authors take a weighted mean of all the other WiFi measurements from the same wireless access points. Formally, this is:

Where Β is the weights, and Z is the WiFi measurements from the access point. The value of Β for the WiFi measurement being predicted needs to be set to 0. This is to insure that the real measurement at point i is not being used to predict the measurement at point i.

The weights for this weighted mean are generated using the following function:

This function is a gaussian kernel and essentially gives the probability that two WiFi measurements are near each other and only separated by free space. X_i and X_j are the believed XY positions of the two WiFi measurements, and Tau is a constant representing the average distance between walls. The paper used 2 meters for this but shows that the value does not matter too much within reason.

There are three important things to note about these weights. The first is that they are based on euclidean distance between WiFi measurements. This means that you must convert out of the Distance-Angle position representation to and X-Y position representation to calculate the weights.

The second is that these weights can not be used to directly make the Beta vector. Before multiplying the Beta vector by the WiFi measurements vector, Beta needs to be normalized so that it sums to one. This makes the weights valid for the weighted mean.

The the third is since we are averaging WiFi signal strengths, the signal strengths themselves must be in units of mW or Watts, not dBm.

The question of whether this WiFi measurement function is valid for use in SLAM will mostly be left as an exercise to the reader. In general, this sort of gaussian kernel for signal strength prediction is supported by the literature. We additionally present some basic and non-rigorous tests of it in our Testing section. We do believe that this measurement function technique is interesting, and worth further research, but are still not 100% convinced of its efficacy.

Solving Graph SLAM

The most important point in understanding the author's graph SLAM solution is understanding how they represent their state space. Remember, the state space is the robot's beliefs about the world. Traditionally, the state space is represented as a matrix with each row corresponding to a time step where measurements were taken, and each column to a different state space variable. An example can be seen below:

In this matrix each row corresponds to a time step where distance, gyroscope, and WiFi measurements were taken: the first column is the believed distance at this time step, the second is the believed angle, and the rest are the believed WiFi signal strengths.

This is not how the authors chose to represent their state space. To the best of our understanding, they formulated their state space as a single column vector. The first set of rows in the column are the distance measurements, the second are the angles, and the remaining rows are the WiFi measurements. This is essentially taking the matrix shown above, cutting the columns out, and stacking them on top of each other as shown below:

Once the state space is formulated in this way, the authors proceed to solve Graph SLAM through the Gauss-Newton algorithm, which is a technique for solving non-linear least squares problems. The formulation for this problem can be seen below:

Here, X₀ is the initial state space estimate and Δ is a correction to it. Δ is found through solving the shown matrix system. The matrix on the far left is the Jacobian of the predicted measurement functions with respect to the state space. The way it is represented above shows that each row in the Jacobian is the derivative of the proper measurement function with respect to all the state space variables. The vector on the far right is the difference between the true measurements and the predicted measurements from the current state space belief.

The calculation of the predicted WiFi measurements was covered in exhausting detail above. Calculating predicted distance measurements is trivial because the state space is already defined in terms of distance. This means that state space distance can simply be subtracted from the measured distance. Calculating predicted gyroscope measures is straightforward and can be seen below:

This prediction technique implicitly assumes that all the angles have equal time steps between them, but this is a good assumption.

The inverse sigmas seen on each side of the equation correspond to the inverse standard deviation of the sensor measurement noise.

Before explaining how to take the measurement function derivatives to create the Jacobian matrix, a note on dimensionality of this equation. Δ must be the same size as the state space so that it can be added to it. The vector of differences between expected and true measurements on the right side must be equal to the number of measurements. Due to the definition of the gyroscope measurement prediction function, there must be one more angle in the state space than gyroscope measurements. To keep things balanced, there also must be one more distance in the state space than distance measurements (we put this extra distance at the end and set it to zero). This means that the Jacobian matrix on the far left is not square; it has a number of rows equal to the number of measurements, and a number of columns equal to the number of state space values. This is necessary for the matrix multiplication to be possible.

To get the derivatives for the Jacobian function, we need the general derivative for the three types of measurement prediction function. Which general derivative is used in which row of the Jacobian is determined by which measurement function was used in the corresponding row of the real minus prediction vector. Each row of the Jacobian matrix is different, because each corresponds to a different measurement, so different values are plugged into the corresponding general derivative function.

The derivative of the distance prediction function with respect to the state variables is easy. It is a row vector where all the elements are zero, except for the one corresponding to the distance state being compared to the distance measure. This element is 1.

The derivative of the angular velocity function is similarly straightforward. The row vector is all zeros except for two elements. The one corresponding to the i angle is -1 and the one corresponding to the i+1 angle is +1.

The derivative of the WiFi prediction function is messy. As noted in the WiFi prediction section, the WiFi prediction must be solved with the state space in terms of X-Y positions for the euclidean distance, but the state space is represented and stored in terms of distances and angles. This means that to take the derivative of the WiFi prediction function with respect to the distance and angle state space, the Jacobian of the distance angle state positions must be found and multiplied by the derivative of the WiFi prediction function taken with respect to the X-Y positions.

The necessary derivatives and Jacobians are shown below. The result is a row vecotr with a number of collums each to the number of state space variabels. Note that the distance-angle state space Jacobian includes a large number of derivatives taken with respect to WiFi measures. All of these derivatives are zero.

After solving for Δ and correcting the state space estimate, this whole matrix system is solved again using the new state space estimate. This process repeats until the state space estimate converges, where convergence is the change to the state space on each iteration passes below a threshold.

When the state space does converge, the final set of distance and angles is the path of the robot.

---

We are only interested in two things: identifying distinct access points across collection samples, and determining their corresponding signal strength. Thus, to filter out useless information, we pipe this command into an egrep as follows: sudo iwlist wlan0 scan | egrep “Cell|Signal”

Each “Cell” line is a different access point reading, so we must collect information about each “Cell” in the output. This line also contains the access point’s MAC address, which is critical to uniquely identifying all access points that we see. We use the access point's MAC address instead of the SSID (network name) of the network because multiple access points can broadcast the same SSID. The line containing “Signal” contains the signal strength in dBm from that access point. The above command yields access point information in the following condensed format:

        
Cell 01 - 	Address: 28:80:88:F2:98:AA
		Quality=45/70  Signal level=-65 dBm
Cell 02 - 	Address: A8:9A:92:A1:9B:89
		Quality=61/70  Signal level=-42 dBm
...
          
      

This format can be easily parsed with regular expressions to determine the MAC address and signal strength of each access point from each collection.

The method of collection is simple. We create a 2D list, where the first element of each entry is the timestamp of collection, and the second is the actual string output from the command.

After collection is finished, we loop through the entire list and extract the useful information with regular expressions. The targeted information is highlighted below:

    
Cell 01 - 	Address: 28:80:88:F2:98:AA
		Quality=45/70  Signal level=-65 dBm
      
  

By the end of the script, a CSV file is created, where the first column is a timestamp, and the remaining columns correspond to readings from specific access points. Each row represents all of the signal strengths taken in that timestamp. This means that each column after the time column represents all of the readings from a single access point; if a signal was not received by a specific access point anytime during collection (after the access point was “seen” earlier), we fill the space with a Not-A-Number.

The script functions with the same GPIO setup as the IMU code, so that when they are launched in parallel in the control script, when button 17 is pressed, both the WiFi script and the IMU script start collecting. This is explained in more detial in the Integration section.

WiFi Data Collection Testing

One major issue we first encountered when using this command was how frequently we would be able to poll the adapter for information. We found that polling as fast as possible would throw an error saying the receiver was “busy” or “didn’t support scanning.” We found that the amount of time spent each loop iteration updating the data collection list mentioned earlier was normally sufficient time to delay each command execution. We found that we could collect WiFi data at a max of around 0.75 - 1 Hz. We also implemented a try/except block such that the code would not immediately quit if the receiver threw an error. Instead, the program would simply move on and continue collecting measurements.

Data Association

After both IMU and WiFi data are collected, we must format the data into matrices that the SLAM algorithm will accept. Two steps are taken to format the data. First, we consolidate the IMU data such that there is only one set of IMU measurements per set of WiFi measurements. This is due to the way the SLAM algorithm is implemented. IMU data can be collected at roughly 20 Hz, whereas the WiFi data is collected only at about 1 Hz. For every WiFi measurement time, we sum all of the distances recorded by the IMU since the last WiFi collection, and we average all of the angular velocities recorded since the last WiFi collection.

Second, we scrap the timestamps for all of the data. This is done after the first step because the time stamps are nessary in that step.

WiFi SLAM Implementation

We chose to implement WiFi SLAM in a way as similar to the math formulation as possible. This was done to make it easier to write the initial code and to validate what was put forward in the paper. Writing the code in a highly analogous way to the math was not good for computational performance, as the easiest way for a human to solve a math problem is rarely the easiest way for a computer to solve it. For example we re-arranged and transposed matrices multiple times in a way that helps when solving by hand, but is not strictly necessary to do. Our plan was to get the algorithm working with poor performance, validate it, and then work on performance improvements.

We did not get the chance to make these performance improvements, or even rigorously profile the code and determine which funcions were causing bottle necks. For datasets that were around a minute in length the SLAM algorithm converged in a matter of seconds. When we tested a ten minute data set, the algorithm took over ninety minutes to converge. We believe that this poor performance can largely be attributed to the size of the matrices associated with larger datasets. Keeping all the matrices in memory at once may not be possible on the Raspberry Pi, leading to large amounts of time spent swapping. Additionally there are multiple calculations in the algorithm, such as calculating the Jacobian matrix, that could be performed in parallel, but were not.

From a software architecture perspective, we implemented the algorithm within a class. The class had a main function that generated the matrices for the matrix equation that needed to be solved, solved the equation, and updated the state vector. It generated the matrices by calling into other class functions that performed the various steps in the algorithm, such as generating the weight vector, generating WiFi predictions, or assembling the jacobian matrix.

As far as the implementation, we made heavy use of NumPy. NumPy is essentially the standard for mathematical computing in Python. One of it's benifits is that is has many useful functions relating to linear algebra. In order to solve the matrix algebra problem presented in the WiFi SLAM section, we used the NumPy linear algebra least square solver. This worked well in that it always gives results regardless of the data in the matrices given, however it may not have been the most appropriate choice for this problem. Outside of performance improvements, an area of future work for this algorithm would be to investigate linear algebra matrix solvers more closely.

Finally, the algorithm was given a maximum of five iterations to converge. It never hit this limit without converging on a solution. The convergence criteria was set as all the state space variables changed by less than 0.1. This was an arbitrary criteria and deserves more research.

Dead Reckoning

Dead reckoning is the process of estimating location based only on movement data. For this project, we need dead reckoning techniques for two reasons. The first is to get the distance we traveled in each time step, which is an input to WiFi SLAM. The second is to generate trajectories we can compare to the outputs of WiFi SLAM. The distances are from the IMU accelerometer data. The full trajectories are from combining the accelerometer data with the gyroscope data. 

Acceleration is the second time derivative of displacement. This means to get distance measures from an accelerometer, we must integrate twice with respect to time. This yields the basic kinematics equation shown below.

V is the velocity and X₀ is the initial displacement. Velocity comes from integrating accelerometer data once and adding the initial velocity. We solve the above equation for each acceleration measurement. In the first iteration, we take the initial displacement and initial velocity to be zero. In every subsequent iteration, we take the initial displacement and velocity to be the value found in the previous iteration.

Getting angles from gyroscope data is a similar process. Gyroscopes give angular velocity, which is the first time derivative of angle. To retrieve the angle, we simply integrate the gyroscope data once with respect to time. The resulting equation can be seen below. Again we take the first initial angle to be zero.

While the above dead reckoning formulation is simple and based on 101 physics, it does not yield good results in the real world. This is because of error in the sensor. The error accumulates through these integrations over time and is never corrected for. This causes the dead reckoning trajectory to diverge from the true trajectory. More advanced approaches to dead reckoning, such as the use of Kalman filters, can be used to improve results.

In our implementation, we solved dead reckoning with the naive approach shown above. Furthermore, we only used one axis of accelerometer data, and one axis of gyroscope data for our results. This simplified the math at the expense of accuracy.

We took several steps to improve the accuracy of our results. First, we applied a moving average filter to our acceleration data. For each acceleration measurement, we averaged it with the three measurements before and the three measurements after. This smoothed out the input data, which could be quite noisy, as seen below:

Second we averaged the first ten acceleration measurements and subtracted this value from all the other measurements. This was done to correct for any normal force acceleration we may have seen.⁴ This subtraction generally worked well, but if the angle of the IMU shifted, the magnitude of the normal force on each axis would change, removing the effects of this subtraction.

Dead Reckoning Testing

To test and validate our dead reckoning results, we generated a number of statistics and plots. These statistics and plots first helped confirm that the data taken from the IMU data collection code was valid. They also confirmed that the dead reckoning was performing as expected. A set of statistics can be seen below:

Number of data reads: 658
Time accounted for: 35.245476288000006
Max time between readings: 0.06054765599999712
Average time between readings: 0.053726282480974136
Meadian time between readings: 0.05368802099999925

These show that the sampling frequency and timing information of the data set is correct. They also give information about the number of samples.

We then plotted the raw accelerometer and gyroscope measurements. This helped us insure that the sensors were giving sensibel information. Example plots are shown below:

Raw accerometer and gyroscope data

Finally, we made plots of the systems velocity, displacment, and trajectory (X-Y positions of the system in space, assuming the initial measurement is the origin). We compared these results to baselines we took by moving the system around and measuring the distance we moved with a tape measure. Example plots are shown below, all plots in this section were generated with the matplotlib Python library.

Example Velocity, Displacment, and Trajctory Plots

We subsequently realized we could not carry the system in our hands to get good results. We could not hold the system level enough for our normal force correction to work. Additionally, the acceleration seen by a foot fall while walking could be seen in all three axes, and our filtering could not compensate for that. Because of this, we took all data sets by either dragging the system around (as seen in the video) or by putting it on something with wheels.

The Mystery of the Missing Time

At one point, while testing the dead reckoning, we noticed that we would get drastically incorrect results. After investigating, we discovered that sometimes our timestamps would jump many seconds. Below is a plot of time plotted against itself from one of these jumps.

Since the code was set up to sample at 20Hz, these jumps were unexpected. Since the dead reckoning relies on time, it would set a constant acceleration for the duration of the time jump and greatly alter our results. We determined that the cause of these jumps was how we were getting times for our timestamps. We were using the python function time.time(), which returns the UNIX time. However immediately after booting the Raspberry Pi this time would be incorrect and then jump to the correct time when the Pi connected to the internet and could contact a time server.

To solve this problem, we switched from using time.time() to using time.clock_gettime(time.CLOCK_MONOTONIC_RAW). What this new function does is return the time in seconds since the system booted. This is not subject to corrections from the time server. We are able to use it because the only metric that is important to our analysis is the time between samples, not the wall clock time.

TFT Plotting

In this script, we use the pygame library along with GPIO to enable the dead reckoning and SLAM plots to be viewed and toggled between on the TFT screen. The plots are output from the dead reckoning code and the SLAM code as png files. We used the techniques we practiced in lab sessions "blit", in pygame parlence, these png files to the screen. Rather, Would begin by displaying one of these plots (the SLAM plot). The user would be able to toggle between this plot and the dead reckoning plot by using buttons 22 and 23 on the TFT. The plots may be scrolled through as a wrap-around list. The wrap around list formuation was used to make it easy to add more plots in the future, if we choose to do so.

TFT Testing

The only issue we had with displaying plots on the TFT was that sometimes they would be different sizes, so on different runs of the program, the screens would have varying sizes of black margin on the borders of the screen. The size diffrences in the plots was caused by matplotlib. In that library plot sizes are spefied in units of inches, but this does not garuntie the same pixel size plot ouputs. We fixed this in pygames by 1) centering each plot and 2) making the background white. This makes the display look much cleaner.

Integration

We launch all of the scripts above in a single control script. We utilize the multiprocessing library to run the two data collection scripts in parallel, using a pool of workers. In this control script, we also pass filenames and file paths to different scripts when we call them for execution. We first create a directory name that the data will be written to at the beginning of the file. We pass this directory name to the data collection scripts. The scripts then write their respective data CSV files to this directory. Next we pass the file paths of the data files to the data association script. The data association script returns two matrices: measurement data and WiFi data. These matrices can be passed inside the control script to the WiFi SLAM script. The WiFi SLAM script and dead reckoning script both output their png plots to the same directory used throughout the run. These plot file paths are finally passed to the TFT script.

Integration Testing

We had some troubles with using the right file paths and passing them correctly to different scripts. This was worked out rather quickly though through trial and error.

The only other problem we had was that the buttons for scrolling through TFT plots were not working when the TFT script was integrated into the control script. This was because in the TFT script all of the GPIO setup was being done outside of the class. Thus, the GPIO setup was being executed upon import of the TFT file into the control script. This meant all of the TFT GPIO was being cleaned up once the data collection scripts called GPIO.cleanup(). To solve this, we simply moved all of the GPIO setup into the TFT class init method.

---

---

---

---

---

---

---

---

---